/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_INTERPOLATABLESENSOR_H_
#define __MMM_INTERPOLATABLESENSOR_H_

#include "../MMMCore.h"
#include "../MMMImportExport.h"
#include "BasicSensor.h"
#include "SensorMeasurement.h"
#include "InterpolatableSensorMeasurement.h"

#include <string>
#include <map>

namespace MMM
{

// M : SensorMeasurement, Cloneable<M>, Interpolated<M>
template <typename M, typename std::enable_if<std::is_base_of<InterpolatableSensorMeasurement<M>, M>::value>::type* = nullptr>
/*! \brief The templated interface with already implemented methods for implementing new interpolatable sensor classes. */
class MMM_IMPORT_EXPORT InterpolatableSensor : public virtual BasicSensor<M> {
public:
    using BasicSensor<M>::getDerivedMeasurement;

    boost::shared_ptr<M> getDerivedMeasurement(float timestep)
    {
        if (BasicSensor<M>::hasMeasurement(timestep)) {
            return this->measurements[timestep];
        } else {
            auto it = this->measurements.lower_bound(timestep);
            if (it != this->measurements.end()) {
                if (it != this->measurements.begin()) {
                    auto measurement = it->second;
                    return (--it)->second->interpolateMeasurement(measurement, timestep);
                }
            }
        }
        return nullptr;
    }

    bool isInterpolatable() {
        return true;
    }

    void synchronizeSensorMeasurements(float timeFrequency, float minTimestep, float maxTimestep) {
        assert(minTimestep >= this->getMinTimestep());
        assert(maxTimestep <= this->getMaxTimestep());
        std::map<float, boost::shared_ptr<M> > newMeasurements;
        if (timeFrequency < 0.001f) return;
        for (float timestep = 0.0f; timestep <= maxTimestep; timestep += timeFrequency) {
            timestep = Math::roundf(timestep, 3);
            if (timestep < minTimestep) continue;
            boost::shared_ptr<M> interpolatedMeasurement = getDerivedMeasurement(timestep);
            if (interpolatedMeasurement) newMeasurements[timestep] = interpolatedMeasurement;
        }
        this->measurements = newMeasurements;
    }

protected:
    InterpolatableSensor(const std::string &description = std::string(), const std::map<float, boost::shared_ptr<M> > &measurements = std::map<float, boost::shared_ptr<M> >()) :
        BasicSensor<M>(description, measurements)
    {
    }
};

}

#endif // __MMM_INTERPOLATABLESENSOR_H_
